#include "plot_aggregator.h"

// 点迹凝聚算法
std::vector<TargetInfo> PlotAggregator::aggregate(const std::vector<DetectionCell> &detections,
                                                  const RadarParameters &params)
{
    if (detections.empty())
    {
        return {};
    }

    std::vector<TargetInfo> targets;
    std::vector<bool> processed(detections.size(), false);

    for (size_t i = 0; i < detections.size(); ++i)
    {
        if (processed[i])
            continue;

        // 创建新目标
        std::vector<size_t> cluster = {i};
        processed[i] = true;

        // 寻找邻近点迹
        for (size_t j = i + 1; j < detections.size(); ++j)
        {
            if (processed[j])
                continue;

            if (isNeighbor(detections[i], detections[j], params))
            {
                cluster.push_back(j);
                processed[j] = true;
            }
        }

        // 计算目标参数
        targets.push_back(computeTargetParameters(cluster, detections, params));
    }

    return targets;
}

// 判断两个点迹是否相邻
bool PlotAggregator::isNeighbor(const DetectionCell &a, const DetectionCell &b,
                                const RadarParameters &params)
{
    float range_diff = std::abs(calculateRange(a.range_bin, params) -
                                calculateRange(b.range_bin, params));
    float velocity_diff = std::abs(calculateVelocity(a.doppler_bin, params) -
                                   calculateVelocity(b.doppler_bin, params));
    float angle_diff = std::abs(calculateAngle(a.angle_bin, params) -
                                calculateAngle(b.angle_bin, params));

    return range_diff <= range_threshold &&
           velocity_diff <= doppler_threshold &&
           angle_diff <= angle_threshold;
}

// 计算目标参数
TargetInfo PlotAggregator::computeTargetParameters(const std::vector<size_t> &cluster,
                                                   const std::vector<DetectionCell> &detections,
                                                   const RadarParameters &params)
{
    float range_sum = 0.0f;
    float velocity_sum = 0.0f;
    float angle_sum = 0.0f;
    float power_sum = 0.0f;
    float max_power = -std::numeric_limits<float>::max();

    for (size_t idx : cluster)
    {
        const auto &det = detections[idx];
        float range = calculateRange(det.range_bin, params);
        float velocity = calculateVelocity(det.doppler_bin, params);
        float angle = calculateAngle(det.angle_bin, params);

        range_sum += range;
        velocity_sum += velocity;
        angle_sum += angle;
        power_sum += det.power;
        max_power = std::max(max_power, det.power);
    }

    int count = cluster.size();
    float avg_range = range_sum / count;
    float avg_velocity = velocity_sum / count;
    float avg_angle = angle_sum / count;
    float avg_power = power_sum / count;

    // 计算SNR (简化计算)
    float noise_floor = avg_power - 10.0f; // 假设噪声比平均功率低10dB
    float snr = max_power - noise_floor;

    return TargetInfo(avg_range, avg_velocity, avg_angle, avg_power, snr, count);
}
